#include "global.h"

/**
 * @brief 限幅函数，确保16位整数值在指定的范围内
 *
 * 该函数用于将一个16位整数值限制在一个指定的上下限范围内。
 * 如果值小于下限，则将其设置为下限；如果值大于上限，则将其设置为上限。
 *
 * @param value 指向需要限制的16位整数值的指针
 * @param lowerLimit 下限值
 * @param upperLimit 上限值
 */
void LIMIT_I16(int16_t *value, int16_t lowerLimit, int16_t upperLimit)
{
    if (*value < lowerLimit)
    {
        *value = lowerLimit; // 如果值小于下限，设置为下限
    }
    else if (*value > upperLimit)
    {
        *value = upperLimit; // 如果值大于上限，设置为上限
    }
}
/**
 * @brief 限幅函数，确保值在指定的范围内
 *
 * 该函数用于将一个浮点数值限制在一个指定的上下限范围内。
 * 如果值小于下限，则将其设置为下限；如果值大于上限，则将其设置为上限。
 *
 * @param value 指向需要限制的浮点数值的指针
 * @param lowerLimit 下限值
 * @param upperLimit 上限值
 */
void LIMIT_F(float *value, float lowerLimit, float upperLimit)
{
    if (*value < lowerLimit)
    {
        *value = lowerLimit; // 如果值小于下限，设置为下限
    }
    else if (*value > upperLimit)
    {
        *value = upperLimit; // 如果值大于上限，设置为上限
    }
}

/**
 * @brief 计算两个角度之间的最小差值
 *
 * 此函数计算两个角度（单位：度）之间的误差，并将结果调整到 [-180°, 180°] 范围内。
 * 这对于处理角度周期性问题非常有用，例如无人机或机器人中的方向控制。
 *
 * @param target 当前角度（单位：度）（范围：-180°到180°）
 * @param measured 目标角度（单位：度）（范围：-180°到180°）
 * @return 修正后的角度误差（范围：-180°到180°）
 */
float wrapAngleDifference(float target, float measured)
{
    // 计算初始角度差
    float error = target - measured;

    // 调整误差到 [-180°, 180°] 范围
    if (error > 180.0f)
    {
        error -= 360.0f;
    }
    else if (error < -180.0f)
    {
        error += 360.0f;
    }
    return error;
}
